from mcap.reader import make_reader
import cv2
import numpy as npkisara
import struct

from data_asst.mcap_utils.mcap_base_reader import FloatArray

mcap_path = "/home/kisara/data/Fnii_vla/data/mcap/pick_cup_1018/0.mcap"

# ------------------- 检查话题内容 -------------------
target_joint_topic = "/right/lead/arm/joint_state/position"
target_eef_topic = "/right/lead/arm/pose/orientation"  # xyzw

first_joint_data = None
first_eef_data = None
with open(mcap_path, "rb") as f:
    reader = make_reader(f)
    for _, channel, message in reader.iter_messages():
        if channel.topic == target_joint_topic and first_joint_data is None:
            first_joint_data = FloatArray.GetRootAs(message.data, 0).ValuesAsNumpy()
        if channel.topic == target_eef_topic and first_eef_data is None:
            first_eef_data = FloatArray.GetRootAs(message.data, 0).ValuesAsNumpy()
        if first_joint_data is not None and first_eef_data is not None:
            break
print(f"First frame joint positions ({target_joint_topic}):")
print(first_joint_data)
print(f"\nFirst frame end-effector orientation ({target_eef_topic}):")
print(first_eef_data)

# ------------------- 查看话题名字 -------------------
topics = set()
attachments = []

with open(mcap_path, "rb") as f:
    reader = make_reader(f)
    # 遍历消息收集 topic 名
    for _, channel, _ in reader.iter_messages():
        topics.add(channel.topic)
    # 遍历附件收集附件名
    for attachment in reader.iter_attachments():
        attachments.append(attachment.name)

print("Topics in file:")
for t in sorted(topics):
    print(" ", t)

print("\nAttachments in file:")
for a in sorted(attachments):
    print(" ", a)
